#include "timer.hpp"

#include "map/map_util.h"
#include <p2p_util/data_utils.h>

#include <planner/planner.h>

using namespace sz;

int main(int argc, char **argv) {

  if (argc != 3) {
    printf(ANSI_COLOR_RED "Input yaml required!\n" ANSI_COLOR_RESET);
    return -1;
  }
  std::shared_ptr<NavigationMap> map_util =
      std::static_pointer_cast<NavigationMap>(LoadMap(std::string(argv[1]), std::string(argv[2])));
  if(!map_util)
    return -1;
  Point2f start(-17.5, 0);
  Point2f goal(17.5, 0);

  cv::Mat map_data = ToMat(*map_util);
  cv::cvtColor(map_data, map_data, CV_GRAY2BGR);
  auto start_map = map_util->WorldToMap(start);
  auto goal_map = map_util->WorldToMap(goal);
  cv::circle(map_data,{start_map.x(), start_map.y()},2,{100, 255, 255}, 1);
  cv::circle(map_data,{goal_map.x(), goal_map.y()},2,{100, 100, 255}, 1);
  auto path_dist = Planner::Plan(map_util, start, goal);

  if (path_dist.size() > 2)
    for (size_t i = 0; i < path_dist.size() - 1; ++i) {
      auto point_map0 = path_dist[i];
      auto point_map1 = path_dist[i + 1];
      cv::circle(map_data,{point_map0.x(), point_map0.y()},0,{255, 255, 100}, 1);
    }

  cv::imshow("Map", map_data);
  cv::waitKey();

  return 0;
}
